%%  Clear workspace & load userpath

clear all;
close all;
clc;

addpath(userpath_matlab);

%%  Method Parameters

rho             = 1;
poliBaseType    = '';
order           = 2;    % 1 or 2
samplesPerNode  = 4;
isUniform       = true;
parallelization = true;

domainDimension = [8, 8];
quantityOfNodes = [16, 16];
center  = [0.5*domainDimension(1); 0.5*domainDimension(2)];

if(isUniform)
    [xNodes hx hy] = UniformGrid2D(quantityOfNodes(1), quantityOfNodes(2),...
        domainDimension(1), domainDimension(2), center);
    [xSamples hxSamples hySamples] = UniformGrid2D(quantityOfNodes(1)*samplesPerNode,...
        quantityOfNodes(2)*samplesPerNode,...
        domainDimension(1), domainDimension(2), center);
else
    xNodes = NonUniformGrid2(2, quantityOfNodes(1), domainDimension(1));
    xSamples = NonUniformGrid2(2, quantityOfNodes(1)*samplesPerNode,...
        domainDimension(1));
end

if(parallelization)
    matlabpool open 8;
end
%%  Generate Form Functions
tic
totalNodes = size(xNodes,1);
totalSamples = size(xSamples,1);
dimension = size(xSamples,2);
baseDimension = getPolibaseDimension(order);

samplesShapeFunction = zeros(totalSamples,totalNodes);
gradSamplesShapeFunction = zeros(totalSamples,totalNodes,dimension);

parfor indexSample = 1:size(xSamples,1)

    %   Inner loop initialization
    gradAlpha = zeros(baseDimension,dimension);
    gradN = zeros(totalNodes,dimension);
    weights = zeros(totalNodes,1);

    for indexNode=1:totalNodes
        distance = norm(xSamples(indexSample) - xNodes(indexNode,:));
        weights(indexNode) = gaussianWeight (distance , rho);
    end
    B = assemblyBMatrix( xSamples(indexSample), xNodes, order , weights , poliBaseType);
    A = assemblyMomentMatrix ( xSamples(indexSample), xNodes, weights , poliBaseType, order );
    p = poliBase (xSamples(indexSample,:),order,poliBaseType);
    alpha = solveAlpha (A,p);
    N = alpha * B';

    samplesShapeFunction(indexSample,:) = N;

    gradP = gradPoliBase (xSamples(indexSample,:),order,poliBaseType);
    gradWeights = gradGaussianWeight (xNodes,xSamples(indexSample,:),rho);
    gradA = assemblyGradMomentMatrix (xSamples(indexSample,:), xNodes,...
        gradWeights , poliBaseType , order);
    loadVectorGradAlpha = assemblyLoadVectorGradAlpha ...
                        (gradP(:,1), gradP (:,2),...
                        gradA(:,:,1), gradA(:,:,2), alpha );

    for d = 1:dimension                    
        [dA,~,~] = solveAlpha(A,loadVectorGradAlpha(:,d)');
        gradAlpha(:,d) = dA;
    end

    gradB = assemblyGradBMatrix(xSamples(indexSample,:),xNodes,order,...
        gradWeights,poliBaseType);

    for d = 1:dimension
        gradN(:,d) = gradAlpha(:,d)' * B' +  alpha * gradB(:,:,d)';
    end
    
    gradSamplesShapeFunction(indexSample,:,:) = gradN; 
end
toc

if(parallelization)
    matlabpool close;
end

%%  Zero Order Consistency

ZeroCheck = 0;

for indexSample = 1:size(xSamples,1)
    ZeroCheck = ZeroCheck + sum(samplesWeights(indexSample,:));
end

ZeroCheck - size(xSamples,1)

%%  First Order Consistency

FirstCheck = zeros(totalSamples,2);

for indexSample = 1:size(xSamples,1)
    FirstCheck(indexSample,:) = samplesWeights(indexSample,:)*xNodes;
end

diffCheck = FirstCheck - xSamples;
max(max(diffCheck))

%%  Second Order Consistency

SecondCheck = zeros(totalSamples,2);

for indexSample = 1:size(xSamples,1)
    SecondCheck(indexSample,:) = samplesWeights(indexSample,:)*xNodes.^2;
end

diffCheck = SecondCheck - xSamples.^2;
max(max(diffCheck))

%%  Plot the grid

scatter(xNodes(:,1),xNodes(:,2),'r','filled'); hold on;
scatter(xSamples(:,1),xSamples(:,2),'Marker','+');

%% Plot the shape functions for a given node
node = xNodes(6, :);
x_samp = xSamples(:,1);
y_samp = xSamples(:,2);

indexNearest = zeros(size(xSamples, 1), size(xNodes, 1));
distanceNearest = zeros(size(xSamples, 1), size(xNodes, 1));
for sample = 1 : size(xSamples, 1);
    for node = 1 ; size(xNodes,1);
        indexNearest(sample, node) = node;
        distanceNearest = norm(xSamples(sample,:), xNodes(node,));
    end;
end;
s_near = [indexNearest distanceN];

[Mat_efg] = preplotShap(p_samp, s_near, out, x_samp, y_samp, xSamples);
plot_domain(boundary, x_nodes);
for ij=length(out):-1:1
  i=out(ij);
  Ma_efg(:,:)=Mat_efg(ij,:,:);
  hold on
  surf(x_samp, y_samp, Ma_efg','FaceColor','white',  ...
    'EdgeColor','none','FaceLighting','phong')
end
view([-145 22])
axis normal
axis([min(x_nodes(:,1)) max(x_nodes(:,1)) min(x_nodes(:,2)) max(x_nodes(:,2)) ...
  -.1 1 ])
title 'Shape function - EFG'
xlabel 'x'
ylabel 'y'
camlight


% Plot the gradients of the shape functions
[dMatdx_efg, dMatdy_efg] = preplotDShap(dp_samp, s_near, out, x_samp, y_samp, x_samples);
plot_domain(boundary,x_nodes);
z_min = 0;
z_max = 0;
for ij=length(out):-1:1
  i=out(ij);
  Ma_lme(:,:) = dMatdx_lme(ij,:,:);
  hold on
  surf(x_samp,y_samp,Ma_lme','FaceColor','white',  ...
    'EdgeColor','none','FaceLighting','phong')
  z_min = min([z_min min(min(Ma_lme))]);
  z_max = max([z_max max(max(Ma_lme))]);
end
view([-130 20])
axis normal
axis([min(x_nodes(:,1)) max(x_nodes(:,1)) min(x_nodes(:,2)) max(x_nodes(:,2)) z_min z_max])
title 'gradient component in x'
xlabel 'x'
ylabel 'y'
camlight

plot_domain(boundary,x_nodes);
z_min = 0;
z_max = 0;
for ij=length(out):-1:1
  i=out(ij);
  Ma_lme(:,:) = dMatdy_lme(ij,:,:);
  hold on
  surf(x_samp,y_samp,Ma_lme','FaceColor','white',  ...
    'EdgeColor','none','FaceLighting','phong')
  z_min = min([z_min min(min(Ma_lme))]);
  z_max = max([z_max max(max(Ma_lme))]);
end
view([-130 20])
axis normal
axis([min(x_nodes(:,1)) max(x_nodes(:,1)) min(x_nodes(:,2)) max(x_nodes(:,2)) z_min z_max])
title 'gradient component in y'
xlabel 'x'
ylabel 'y'
camlight

